{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "%pylab inline\n",
    "from sympy import init_printing,Matrix\n",
    "init_printing(use_latex='mathjax')\n",
    "from scipy import weave\n",
    "import os\n",
    "import cv2\n",
    "import pickle"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 从图像提取直线"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 利用骨骼细化提取目标直线轮廓\n",
    "def _thinningIteration(im, iter):\n",
    "    I, M = im, np.zeros(im.shape, np.uint8)\n",
    "    expr = \"\"\"\n",
    "    for (int i = 1; i < NI[0]-1; i++) {\n",
    "        for (int j = 1; j < NI[1]-1; j++) {\n",
    "            int p2 = I2(i-1, j);\n",
    "            int p3 = I2(i-1, j+1);\n",
    "            int p4 = I2(i, j+1);\n",
    "            int p5 = I2(i+1, j+1);\n",
    "            int p6 = I2(i+1, j);\n",
    "            int p7 = I2(i+1, j-1);\n",
    "            int p8 = I2(i, j-1);\n",
    "            int p9 = I2(i-1, j-1);\n",
    "            int A  = (p2 == 0 && p3 == 1) + (p3 == 0 && p4 == 1) +\n",
    "                     (p4 == 0 && p5 == 1) + (p5 == 0 && p6 == 1) +\n",
    "                     (p6 == 0 && p7 == 1) + (p7 == 0 && p8 == 1) +\n",
    "                     (p8 == 0 && p9 == 1) + (p9 == 0 && p2 == 1);\n",
    "            int B  = p2 + p3 + p4 + p5 + p6 + p7 + p8 + p9;\n",
    "            int m1 = iter == 0 ? (p2 * p4 * p6) : (p2 * p4 * p8);\n",
    "            int m2 = iter == 0 ? (p4 * p6 * p8) : (p2 * p6 * p8);\n",
    "            if (A == 1 && B >= 2 && B <= 6 && m1 == 0 && m2 == 0) {\n",
    "                M2(i,j) = 1;\n",
    "            }\n",
    "        }\n",
    "    }\n",
    "    \"\"\"\n",
    "\n",
    "    weave.inline(expr, [\"I\", \"iter\", \"M\"])\n",
    "    return (I & ~M)\n",
    "\n",
    "def thinning(src):\n",
    "    dst = src.copy() / 255\n",
    "    prev = np.zeros(src.shape[:2], np.uint8)\n",
    "    diff = None\n",
    "\n",
    "    while True:\n",
    "        dst = _thinningIteration(dst, 0)\n",
    "        dst = _thinningIteration(dst, 1)\n",
    "        diff = np.absolute(dst - prev)\n",
    "        prev = dst.copy()\n",
    "        if np.sum(diff) == 0:\n",
    "            break\n",
    "\n",
    "    return dst * 255"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 用图像金字塔等提速,获得图像直线的齐次表示\n",
    "def getLine(src):\n",
    "    dst = cv2.cvtColor(src, cv2.COLOR_BGR2GRAY) # 灰度图\n",
    "    dst = cv2.pyrDown(dst) # 降阶一次\n",
    "    dst = cv2.pyrDown(dst) # 降阶两次\n",
    "    retval,dst = cv2.threshold(dst,10,255,cv2.THRESH_BINARY_INV) # 二值化\n",
    "    dst = cv2.morphologyEx(dst, cv2.MORPH_OPEN, np.ones((5,5),np.uint8)) # 开运算\n",
    "    dst = cv2.morphologyEx(dst, cv2.MORPH_CLOSE, np.ones((5,5),np.uint8)) # 闭运算\n",
    "    dst = thinning(dst)\n",
    "    lines = cv2.HoughLinesP(dst, 1, math.pi/180.0, 50, np.array([]), 20, 10)\n",
    "    if lines is None: return None\n",
    "    a,b,c = lines.shape\n",
    "    max_dist = 0\n",
    "    line = None\n",
    "    pt = []\n",
    "    for i in range(a):\n",
    "        for j in range(b):\n",
    "            pt1 = lines[i][j][0:2]\n",
    "            pt2 = lines[i][j][2:4]\n",
    "            dist = np.linalg.norm(pt1 - pt2)\n",
    "            if dist > max_dist:\n",
    "                max_dist = dist\n",
    "                pt = [pt1,pt2]\n",
    "                pt1 = pt1*2**2\n",
    "                pt2 = pt2*2**2\n",
    "                pt1 = np.hstack((pt1, 1))\n",
    "                pt2 = np.hstack((pt2, 1))\n",
    "                line = np.cross(pt1, pt2) # 直线的齐次表示\n",
    "    return np.matrix(line.reshape((3,1))) # 直线的齐次表示"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "imp = cv2.imread(os.path.join(\"data\",\"+.jpg\"))\n",
    "impp = cv2.imread(os.path.join(\"data\",\"++.jpg\"))\n",
    "line1 = getLine(imp)\n",
    "line2 = getLine(impp)\n",
    "line1,line2"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "# 三维直线重建"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 相机参数\n",
    "class Cam(object):\n",
    "    def __init__(self,name=None):\n",
    "        self.unit = 20.0 # 标定板单位为 20mm\n",
    "        self.height = 480 # 相机标定时的高\n",
    "        self.width = 640 # 相机标定时的宽\n",
    "        self.ratio = 1 # 图像缩放比\n",
    "        self.tr = None # 从标定板坐标系到相机坐标系的变换矩阵\n",
    "        self.name = name\n",
    "        self.mtx = self.get_cameraMatrix()\n",
    "        self.dist = self.get_distCoeffs()\n",
    "\n",
    "    def __getattr__(self, item):\n",
    "        if item in self.__dict__:\n",
    "            return self.__dict__[item]\n",
    "        if item == \"P\": # 相机矩阵, P_{3x4}=(K_{3,3},0)_{3,4}\n",
    "            # 将本相机坐标系中的三维坐标变换到本相机的二维图像中\n",
    "            K = self.get_cameraMatrix()*self.ratio\n",
    "            C = np.matrix(np.zeros((3,1)))\n",
    "            P = np.hstack((K,C))\n",
    "            return P\n",
    "\n",
    "    def get_cameraMatrix(self):\n",
    "        mtx = np.eye(3)\n",
    "        if self.name == '+':\n",
    "            fx = 1511.64442682\n",
    "            fy = 1514.72927917\n",
    "            cx = 331.982535978\n",
    "            cy = 268.613047699\n",
    "        elif self.name == \"++\":\n",
    "            fx = 1511.64442682\n",
    "            fy = 1514.72927917\n",
    "            cx = 331.982535978\n",
    "            cy = 268.613047699\n",
    "        else:\n",
    "            fx = 1500.0\n",
    "            fy = 1500.0\n",
    "            cx = self.width/2.0\n",
    "            cy = self.height/2.0\n",
    "\n",
    "        mtx[0,0],mtx[1,1],mtx[0,2],mtx[1,2] = fx,fy,cx,cy\n",
    "        return mtx\n",
    "\n",
    "    def get_distCoeffs(self):\n",
    "        dist = np.zeros((1,5))\n",
    "        if self.name == \"+\":\n",
    "            k1 = -0.105356293294\n",
    "            k2 = 3.25067090251\n",
    "            p1 = 0.00155905212496\n",
    "            p2 = -0.00569751368371\n",
    "            k3 = -25.6347944753\n",
    "        elif self.name == \"++\":\n",
    "            k1 = -0.105356293294\n",
    "            k2 = 3.25067090251\n",
    "            p1 = 0.00155905212496\n",
    "            p2 = -0.00569751368371\n",
    "            k3 = -25.6347944753\n",
    "        else:\n",
    "            k1 = 0\n",
    "            k2 = 0\n",
    "            p1 = 0\n",
    "            p2 = 0\n",
    "            k3 = 0\n",
    "\n",
    "        dist[0] = k1,k2,p1,p2,k3\n",
    "        return dist"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 空间直线的 plucker 表示"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from robot import rotx,roty,rotz,tr2rpy,rpy2tr\n",
    "from robot import trotx,troty,trotz,transl"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 空间两点组成一个直线\n",
    "A = np.matrix([1.0,5.0,3.0,1.0],dtype=np.float32).transpose()\n",
    "B = np.matrix([8.0,3.0,7.0,1.0],dtype=np.float32).transpose()\n",
    "L = A*B.transpose() - B*A.transpose() # 空间直线的两点型 Plucker 表示"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def dual_plucker(L):\n",
    "    \"\"\"\n",
    "    plucker 的对偶操作\n",
    "    一般 plucker 表示是用两空间点的齐次坐标表示, L_{4,4} = AB^T - BA^T\n",
    "    但也可以用两空间的平面的齐次坐标表示, L^* = pi1*pi2^T - pi2*pi1^T\n",
    "    两者互为对偶关系: L^* x L = 0, 关系如下:\n",
    "    l_12 : l_13 : l_14 : l_23 : l_42 : l_34 ==\n",
    "    == l^*_34 : l^*_42 : l^*_23 : l^*_14 : l^*_13 : l^*_12\n",
    "    :param L: 空间直线的 plucker 表示矩阵\n",
    "    :return: 相同空间直线的 plucker 表示矩阵的对偶表示矩阵\n",
    "    \"\"\"\n",
    "    dual_L = np.zeros((4,4))\n",
    "    dual_L[0,1] = L[2,3]\n",
    "    dual_L[0,2] = L[3,1]\n",
    "    dual_L[0,3] = L[1,2]\n",
    "    dual_L[1,2] = L[0,3]\n",
    "    dual_L[3,1] = L[0,2]\n",
    "    dual_L[2,3] = L[0,1]\n",
    "    return dual_L - dual_L.transpose()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Lstar = dual_plucker(L)\n",
    "assert(Lstar*A == 0).all() # A 在直线L上\n",
    "assert(Lstar*B == 0).all() # B 在直线L上\n",
    "assert(Lstar*(A+3*(A-B))== 0).all() # 在直线L上"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 直线的标准 plucker 表示\n",
    "\n",
    "定义: 三维欧式空间中一条直线 $r$ 可由直线上一点 $P$ 和单位方向向量 $l$ 确定, 直线L的距向量 $\\bar{l} = P \\times l = (P+\\lambda l)\\times l$,\n",
    "\n",
    "因此距向量 $\\bar{l}$ 与直线 $L$ 上点 $P$ 的选择无关,且垂直于包含坐标原点和直线的平面.\n",
    "\n",
    "定义: 由单位方向向量 $l$ 和距向量 $\\bar{l}$ 组成的6元组 $(l,\\bar{l}) = (l_1,l_2,l_3,l_4,l_5,l_6)$ 称为直线L的标准 Pluker 坐标:\n",
    "\n",
    "\n",
    "引理: 设两条直线 $L$ , $M$ 对应的 Plucker 坐标分别为 $(l_1,l_2,l_3,l_4,l_5,l_6)$ 和 $(m_1,m_2,m_3,m_4,m_5,m_6)$ 则两直线相交的充要条件为:\n",
    "\n",
    "$$\n",
    "l_1m_4 + l_2m_5 + l_3m_6 + l_4m_1 + l_5m_2 + l_6m_3 = 0\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "(A-B)[:3,0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "n = (A-B)[:3,0]/norm((A-B)[:3,0])\n",
    "P = A[:3] + 2.5*n\n",
    "l_bar = np.cross(array(n).flatten(),array(P).flatten())\n",
    "l_bar,array(n).flatten()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 直线的两点式 plucker 表示标准式表示的关系"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Matrix(L)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "A-B"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "min_e = inf\n",
    "best=[]\n",
    "n = A-B\n",
    "for x in [17,37,26,-17,-37,-26]:\n",
    "    for y in [17,37,26,-17,-37,-26]:\n",
    "        for z in [17,37,26,-17,-37,-26]:\n",
    "            l_bar = [x,y,z,0]\n",
    "            e1 = np.dot(l_bar,n)\n",
    "            e2 = np.dot(l_bar,A+0.5*n)\n",
    "            e = e1+e2\n",
    "            if e == 0:\n",
    "                print [x,y,z,1]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "所以,将标准表示变为两点式:\n",
    "\n",
    "$$\n",
    "\\begin{bmatrix}\n",
    "0    & l_6  & -l_5 & l_1 \\\\ \n",
    "-l_6 & 0    & l_4  & l_2 \\\\ \n",
    "l_5  & -l_4 & 0    & l_3 \\\\ \n",
    "-l_1 & -l_2 & -l3  & 0\n",
    "\\end{bmatrix}\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 与 Oxy 平面的交点"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "直接使用 $L*\\pi$ 得到的点并不是直线与 Oxy 平面的交点,"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "W = L*np.matrix([1,1,1,0]).reshape(4,1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "W"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "W = W/W[-1]\n",
    "W"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "M = np.vstack((Lstar,[0,0,1,0]))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "可利用直线平面 plucker 表示的右零空间是直线上点的坐标集合来求解"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "vec = L[:3,3] # 直线的方向向量\n",
    "vec = np.vstack((vec,1))\n",
    "S,V,D = svd(Lstar)\n",
    "D = D.transpose()\n",
    "p1 = D[:,2] # 直线上的一个点\n",
    "p1 = p1/p1[-1] # 齐次化\n",
    "p2 = D[:,3] # 直线上的另一个点\n",
    "p2 = p2/p2[-1] # 齐次化\n",
    "point = p1 - vec*p1[2]/vec[2] # 与 Oxy 平面的交点\n",
    "point = point/point[-1,0] # 齐次化\n",
    "point"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 正向投影研究"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 两相机的位姿\n",
    "tr1 = pickle.load(open(os.path.join(\"data\",\"tr1.pkl\")))\n",
    "tr2 = pickle.load(open(os.path.join(\"data\",\"tr2.pkl\")))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 相机对象\n",
    "camp = Cam(\"+\")\n",
    "campp = Cam(\"++\")\n",
    "camp.tr = tr1\n",
    "campp.tr = tr2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "rvec = cv2.Rodrigues(tr1[:3,:3])[0]\n",
    "tvec = transl(tr1)\n",
    "mtx,dist = camp.mtx,camp.dist\n",
    "pointA = cv2.projectPoints(A[:3,0].reshape((1,3,1)),rvec,tvec,mtx,dist)[0] # 投影到相机1\n",
    "pointA = cv2.undistortPoints(pointA,mtx,dist,np.array([]),mtx) # 畸变矫正\n",
    "Matrix(pointA[0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "pointA = camp.P*tr1*A\n",
    "pointA = pointA/pointA[-1]\n",
    "Matrix(pointA)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 三维直线反向投影"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# A,B 投影到相机1,得到投影直线 line1\n",
    "rvec = cv2.Rodrigues(tr1[:3,:3])[0]\n",
    "tvec = transl(tr1)\n",
    "mtx,dist = camp.mtx,camp.dist\n",
    "pointA = cv2.projectPoints(A[:3,0].reshape((1,3,1)),rvec,tvec,mtx,dist)[0] # 投影到相机1\n",
    "pointB = cv2.projectPoints(B[:3,0].reshape((1,3,1)),rvec,tvec,mtx,dist)[0] # 投影到相机1\n",
    "pointA = cv2.undistortPoints(pointA,mtx,dist,np.array([]),mtx) # 畸变矫正\n",
    "pointB = cv2.undistortPoints(pointB,mtx,dist,np.array([]),mtx) # 畸变矫正\n",
    "pointA = np.hstack((pointA.reshape((2,)),1))\n",
    "pointB = np.hstack((pointB.reshape((2,)),1))\n",
    "line1 = np.cross(pointA,pointB)\n",
    "line1 = line1/line1[-1]\n",
    "line1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# A,B 投影到相机2,得到投影直线 line2\n",
    "rvec = cv2.Rodrigues(tr2[:3,:3])[0]\n",
    "tvec = transl(tr2)\n",
    "mtx,dist = campp.mtx,campp.dist\n",
    "pointA = cv2.projectPoints(A[:3,0].reshape((1,3,1)),rvec,tvec,mtx,dist)[0] # 投影到相机1, 带畸变\n",
    "pointB = cv2.projectPoints(B[:3,0].reshape((1,3,1)),rvec,tvec,mtx,dist)[0] # 投影到相机1, 带畸变\n",
    "pointA = cv2.undistortPoints(pointA,mtx,dist,np.array([]),mtx) # 畸变矫正\n",
    "pointB = cv2.undistortPoints(pointB,mtx,dist,np.array([]),mtx) # 畸变矫正\n",
    "pointA = np.hstack((pointA.reshape((2,)),1))\n",
    "pointB = np.hstack((pointB.reshape((2,)),1))\n",
    "line2 = np.cross(pointA,pointB)\n",
    "line2 = line2/line2[-1]\n",
    "line2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 投影到各自相机坐标系下得到平面的方程\n",
    "pi1 = camp.P.transpose()*line1.reshape((3,1))\n",
    "pi2 = campp.P.transpose()*line2.reshape((3,1))\n",
    "pi1.transpose(),pi2.transpose() # 因为在相机坐标系下的平面过原点,所以平面的齐次表示最后一个值为0正确"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 将投影到相机坐标系下的平面齐次表示变换到另一个坐标系\n",
    "# 需要考虑, 该平面一定过相机的原点,\n",
    "# 实际上就是平面的齐次表示的最后一个值,不应该仍然是 0\n",
    "# 假设变换矩阵为 T_C^W 表示将坐标从 C 下变换到 W\n",
    "# 则 C 的原点在 W 中的表示为 T_C^W * [0,0,0,1]^T\n",
    "# 下述函数中 pi 为平面在相机坐标系 C 下的表示\n",
    "# tr 为从相机坐标系 C 到世界坐标系 W 的变换矩阵, T_C^W\n",
    "def tr4pi(tr,pi):\n",
    "    pi = tr*pi\n",
    "    if pi[-1] == 0:\n",
    "        C = tr[:,3] # C 的原点在 W 中的表示\n",
    "        pi[-1] = -C.transpose()*pi # 确保 C^T*pi=0 即确保相机的原点在新平面pi上\n",
    "        assert (C.transpose()*pi == 0).all()\n",
    "    \n",
    "    #pi = pi/pi[-1] # 齐次化\n",
    "    return pi"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "pi1 = tr4pi(inv(tr1),pi1)\n",
    "pi2 = tr4pi(inv(tr2),pi2)\n",
    "pi1.transpose(),pi2.transpose()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 直线的 Plucker 表示 "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 空间直线的两面 Plucker 表示\n",
    "Lstar = pi1*pi2.transpose() - pi2*pi1.transpose() "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Matrix(Lstar*A)# 直线过 A 点"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Matrix(Lstar*B) # 直线过 B 点"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "S,V,D = svd(Lstar)\n",
    "D = D.transpose()\n",
    "p1 = D[:,2] # 直线上的一个点\n",
    "p1 = p1/p1[-1,0] # 齐次化\n",
    "p2 = D[:,3] # 直线上的另一个点\n",
    "p2 = p2/p2[-1,0] # 齐次化\n",
    "n = p1-p2 # 方向向量\n",
    "n = n/norm(n) # 单位化\n",
    "Matrix(n)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Matrix((A-B)/norm(A-B))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 直线的面表示\n",
    "\n",
    "其二维右零空间为是一条直线 L 构成的空间点束,\n",
    "\n",
    "则svd分解得到的零向量应该就是空间直线的方向向量"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 直线的面表示\n",
    "w = np.vstack((pi1.transpose(),pi2.transpose()))\n",
    "S,V,D = svd(w)\n",
    "D = D.transpose()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "p1 = D[:,2] # 直线上的一个点\n",
    "p1 = p1/p1[-1,0] # 齐次化\n",
    "p2 = D[:,3] # 直线上的另一个点\n",
    "p2 = p2/p2[-1,0] # 齐次化\n",
    "n = p1-p2 # 方向向量\n",
    "n = n/norm(n) # 单位化\n",
    "Matrix(n)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "p = p1 - n*p1[2]/n[2] # 与 Oxy 平面的交点\n",
    "p"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Lstar*p"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 图像直线重建成功!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 如何将直线与点进行相同变换?\n",
    "\n",
    "已知: \n",
    "\n",
    "$$\n",
    "L = A B^T - B A^T\n",
    "$$\n",
    "\n",
    "$$\n",
    "A' = T A\n",
    "$$\n",
    "\n",
    "$$\n",
    "B' = T B\n",
    "$$\n",
    "\n",
    "$$\n",
    "L' = A'B'^T - B'A'^T = (TA)(TB)^T - (TB)(TA)^T = T A B^T T^T - T B A^T T^T = T L T^T\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "A = np.matrix([1.0,5.0,3.0,1.0],dtype=np.float32).transpose()\n",
    "B = np.matrix([8.0,3.0,7.0,1.0],dtype=np.float32).transpose()\n",
    "L = A*B.transpose() - B*A.transpose() # 空间直线的两点型 Plucker 表示\n",
    "Lstar = dual_plucker(L)\n",
    "norm(Lstar*A),norm(Lstar*B)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "delta_tr = rpy2tr(0.5,0.9,0.3)*transl(18,29,78)\n",
    "dA = delta_tr*A\n",
    "dB = delta_tr*B\n",
    "dL =  delta_tr*L*delta_tr.transpose()\n",
    "dLstar = dual_plucker(dL)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "norm(dLstar*dA),norm(dLstar*dB)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "平面表示不能执行此变换:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "dLstar = delta_tr*Lstar*delta_tr.transpose()\n",
    "norm(dLstar*dA),norm(dLstar*dB)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
